#ifndef _BASIC_DEV_CPP_
#define _BASIC_DEV_CPP_

#include "basic_dev.hpp"

//结构体定义
typedef struct _Command
{
  float force[3];
  float qx, qy, qz, qw;//目标姿态角四元数
  float kR[3];
  float kOm[3];//目标三轴角速度
  float corrections[3];
  float current_yaw;//待改
  bool  use_external_yaw;
} Command;//来自SO3命令结构体

typedef struct _IMU
{
  // Header部分
  uint32_t seq;  // 序列号
  time stamp_sec;  // 时间戳
  std::string frame_id;  // 坐标系ID

  // 线性加速度
  float64 linear_acceleration[3];  // x, y, z

  // 角速度
  float64 angular_velocity[3];  // x, y, z

  // 方向（四元数）
  float64 orientation[4];  // x, y, z, w

  // 协方差矩阵(貌似rm那边不会发，应该可以不用)
  float64 orientation_covariance[9];  // 3x3矩阵
  float64 angular_velocity_covariance[9];  // 3x3矩阵
  float64 linear_acceleration_covariance[9];  // 3x3矩阵
} IMU;//来自IMU的数据

typedef struct _Control
{
  double rpm[4];
} Control;//定义四个电机转速结构体


//函数声明
static Control  getControl(const QuadrotorSimulator::Quadrotor& quad, const Command& cmd);

int main(int argc, char** argv)
{

    ros::init(argc, argv, "basic_dev"); // 初始化ros 节点，命名为 basic
    ros::NodeHandle n; // 创建node控制句柄
    BasicDev go(&n);
    while (n.ok())
    {
        ros::spinOnce();

        auto last = control;//记录上次四旋翼无人机电机值
        control   = getControl(quad, command);//计算得到本次四旋翼无人机电机值
        for (int i = 0; i < 4; ++i)
        {
            if (std::isnan(control.rpm[i]))
            control.rpm[i] = last.rpm[i];
        }

        //pwm控制信息发送
        airsim_ros::RotorPWM pwm_cmd;
        pwm_cmd.rotorPWM0 = control.rpm[0];
        pwm_cmd.rotorPWM1 = control.rpm[1];
        pwm_cmd.rotorPWM2 = control.rpm[2];
        pwm_cmd.rotorPWM3 = control.rpm[3];
        pwm_pub.publish(pwm_cmd);//发布控制信息
    }
    return 0;
}

BasicDev::BasicDev(ros::NodeHandle *nh)
{  
    //创建图像传输控制句柄
    it = std::make_unique<image_transport::ImageTransport>(*nh); 
    front_left_img = cv::Mat(480, 640, CV_8UC3, cv::Scalar(0));
    front_right_img = cv::Mat(480, 640, CV_8UC3, cv::Scalar(0));

    takeoff.request.waitOnLastTask = 1;
    land.request.waitOnLastTask = 1;

    // 使用publisher发布速度指令需要定义 Velcmd , 并赋予相应的值后，将他publish（）出去
    // velcmd.twist.angular.z = 0;//z方向角速度(yaw, deg)
    // velcmd.twist.linear.x = 1; //x方向线速度(m/s)
    // velcmd.twist.linear.y = 0;//y方向线速度(m/s)
    // velcmd.twist.linear.z = 0;  //z方向线速度(m/s)

    //无人机信息通过如下命令订阅，当收到消息时自动回调对应的函数
    odom_suber = nh->subscribe<geometry_msgs::PoseStamped>("/airsim_node/drone_1/debug/pose_gt", 1, std::bind(&BasicDev::pose_cb, this, std::placeholders::_1));//状态真值，用于赛道一
    gps_suber = nh->subscribe<geometry_msgs::PoseStamped>("/airsim_node/drone_1/gps", 1, std::bind(&BasicDev::gps_cb, this, std::placeholders::_1));//GPS数据
    imu_suber = nh->subscribe<sensor_msgs::Imu>("airsim_node/drone_1/imu/imu", 1, std::bind(&BasicDev::imu_cb, this, std::placeholders::_1));//imu数据
    lidar_suber = nh->subscribe<sensor_msgs::PointCloud2>("airsim_node/drone_1/lidar", 1, std::bind(&BasicDev::lidar_cb, this, std::placeholders::_1));//lidar数据
    cmd_suber = nh->subscribe<geometry_msgs::Twist>("cmd_vel", 1, std::bind(&BasicDev::cmd_cb, this, std::placeholders::_1));
    front_left_view_suber = it->subscribe("airsim_node/drone_1/front_left/Scene", 1, std::bind(&BasicDev::front_left_view_cb, this,  std::placeholders::_1));
    front_right_view_suber = it->subscribe("airsim_node/drone_1/front_right/Scene", 1, std::bind(&BasicDev::front_right_view_cb, this,  std::placeholders::_1));

    //额外添加的话题订阅
    //ros::Subscriber cmd_sub = n.subscribe("so3_cmd", 100, &cmd_callback, ros::TransportHints().tcpNoDelay());//接收SO3控制命令
    cmd_sub = nh->subscribe<quadrotor_msgs::SO3Command>("so3_cmd", 100, std::bind(&BasicDev::cmd_cb, this, std::placeholders::_1));
    key_cmd_sub = nh->subscribe<geometry_msgs::Twist>("cmd_vel", 1, std::bind(&BasicDev::key_cmd_cb, this, std::placeholders::_1));

    //通过这两个服务可以调用模拟器中的无人机起飞和降落命令
    takeoff_client = nh->serviceClient<airsim_ros::Takeoff>("/airsim_node/drone_1/takeoff");
    land_client = nh->serviceClient<airsim_ros::Takeoff>("/airsim_node/drone_1/land");
    reset_client = nh->serviceClient<airsim_ros::Reset>("/airsim_node/reset");

    //通过publisher实现对无人机的控制
    pwm_publisher = nh->advertise<airsim_ros::RotorPWM>("airsim_node/drone_1/rotor_pwm_cmd", 1);
    //根据读取到的键盘数据，发送速度控制包给SO3解算
    vel_pub = nh->advertise<quadrotor_msgs::PositionCommand>("velocity_cmd", 10);
    
    // takeoff_client.call(takeoff); //起飞
    // land_client.call(land); //降落
    // reset_client.call(reset); //重置
    ros::spin();
}

BasicDev::~BasicDev()
{
}

void BasicDev::pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    Eigen::Quaterniond q(msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z);
    Eigen::Vector3d eulerAngle = q.matrix().eulerAngles(2,1,0);
    ROS_INFO("Get pose data. time: %f, eulerangle: %f, %f, %f, posi: %f, %f, %f\n", msg->header.stamp.sec + msg->header.stamp.nsec*1e-9,
        eulerAngle[0], eulerAngle[1], eulerAngle[2], msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
}

void BasicDev::gps_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    Eigen::Quaterniond q(msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z);
    Eigen::Vector3d eulerAngle = q.matrix().eulerAngles(2,1,0);
    ROS_INFO("Get gps data. time: %f, eulerangle: %f, %f, %f, posi: %f, %f, %f\n", msg->header.stamp.sec + msg->header.stamp.nsec*1e-9,
        eulerAngle[0], eulerAngle[1], eulerAngle[2], msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
}

void BasicDev::imu_cb(const sensor_msgs::Imu::ConstPtr& msg)
{
    ROS_INFO("Get imu data. time: %f", msg->header.stamp.sec + msg->header.stamp.nsec*1e-9);
    // 将ROS消息中的数据复制到结构体中
    imu.seq = msg.header->seq;
    imu.stamp_sec = msg->header.stamp.sec + msg->header.stamp.nsec*1e-9;
    imu.frame_id = msg.header->frame_id;

    // 线性加速度
    imu.linear_acceleration[0] = msg->linear_acceleration.x;
    imu.linear_acceleration[1] = msg->linear_acceleration.y;
    imu.linear_acceleration[2] = msg->linear_acceleration.z;

    // 角速度
    imu.angular_velocity[0] = msg->angular_velocity.x;
    imu.angular_velocity[1] = msg->angular_velocity.y;
    imu.angular_velocity[2] = msg->angular_velocity.z;

    // 方向（四元数）
    imu.orientation[0] = msg->orientation.x;
    imu.orientation[1] = msg->orientation.y;
    imu.orientation[2] = msg->orientation.z;
    imu.orientation[3] = msg->orientation.w;
}

void BasicDev::front_left_view_cb(const sensor_msgs::ImageConstPtr& msg)
{
    cv_front_left_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_8UC3);
    if(!cv_front_left_ptr->image.empty())
    {
        ROS_INFO("Get front left image.: %f", msg->header.stamp.sec + msg->header.stamp.nsec*1e-9);
    }
}

void BasicDev::front_right_view_cb(const sensor_msgs::ImageConstPtr& msg)
{
    cv_front_right_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_8UC3);
    if(!cv_front_right_ptr->image.empty())
    {
        ROS_INFO("Get front right image.%f", msg->header.stamp.sec + msg->header.stamp.nsec*1e-9);
    }
}

void BasicDev::lidar_cb(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
    ;
}

void BasicDev::cmd_cb(const geometry_msgs::Twist::ConstPtr& msg) {
    so3_command.force[0]         = msg->force.x;
  so3_command.force[1]         = msg->force.y;
  so3_command.force[2]         = msg->force.z;
  so3_command.qx               = msg->orientation.x;
  so3_command.qy               = msg->orientation.y;
  so3_command.qz               = msg->orientation.z;
  so3_command.qw               = msg->orientation.w;
  so3_command.kR[0]            = msg->kR[0];
  so3_command.kR[1]            = msg->kR[1];
  so3_command.kR[2]            = msg->kR[2];
  so3_command.kOm[0]           = msg->kOm[0];
  so3_command.kOm[1]           = msg->kOm[1];
  so3_command.kOm[2]           = msg->kOm[2];
  so3_command.corrections[0]   = msg->aux.kf_correction;
  so3_command.corrections[1]   = msg->aux.angle_corrections[0];
  so3_command.corrections[2]   = msg->aux.angle_corrections[1];
  so3_command.current_yaw      = msg->aux.current_yaw;
  so3_command.use_external_yaw = msg->aux.use_external_yaw;
}

void BasicDev::key_cmd_cb(const geometry_msgs::Twist::ConstPtr& msg) {
    velcmd.velocity.x = msg->linear.x;
    velcmd.velocity.y = msg->linear.y;
    velcmd.velocity.z = msg->linear.z;
    velcmd.yaw = msg->angular.z;

    vel_pub.publish(velcmd);

}


//用于根据命令计算无人机四个电机的转速
    // 无人机系统参数
    //     质量 0.9kg
    //     轴距（电机至机体中心）0.18米
    //     转动惯量 Ixx 0.0046890742, Iyy 0.0069312, Izz 0.010421166
    //     电机升力系数 0.000367717
    //     电机反扭力系数 4.888486266072161e-06
    //     最大转速 11079.03 转每分钟

static Control  getControl(const QuadrotorSimulator::Quadrotor& quad, const Command& cmd)
{
  const double _kf = 0.000367717;//电机升力系数
  const double _km = 4.888486266072161e-06;//电机反扭力系数
  const double kf  = _kf - cmd.corrections[0];
  const double km  = _km / _kf * kf;

  const double          d       = 0.18;//四旋翼轴距
  //const Eigen::Matrix3f J       = quad.getInertia().cast<float>();//四旋翼惯性矩阵，由转动惯量获得
  const float           I[3][3] = { { 0.0046890742, 0, 0 },
                                    { 0, 0.0069312, 0 },
                                    { 0, 0, 0.010421166 } };
  //根据当前四元数计算无人机当前旋转矩阵
  flow qw,qx,qy,qz;
  qx = imu.orientation[0];
  qy = imu.orientation[1];
  qz = imu.orientation[2];
  qw = imu.orientation[3];
  float R11 = qw * qw + qx * qx - qy * qy - qz * qz;
  float R12 = 2 * (qx * qy - qw * qz);
  float R13 = 2 * (qx * qz + qw * qy);
  float R21 = 2 * (qx * qy + qw * qz);
  float R22 = qw * qw - qx * qx + qy * qy - qz * qz;
  float R23 = 2 * (qy * qz - qw * qx);
  float R31 = 2 * (qx * qz - qw * qy);
  float R32 = 2 * (qy * .qz + qw * qx);
  float R33 = qw * qw - qx * qx - qy * qy + qz * qz;

  //角速度向量的三个分量，需要重新赋值，待改
   // 角速度
  float Om1 = imu.angular_velocity[0];//x
  float Om2 = imu.angular_velocity[1];//y
  float Om3 = imu.angular_velocity[2];//z

  //根据目标四元数计算无人机目标旋转矩阵
  float Rd11 =
    cmd.qw * cmd.qw + cmd.qx * cmd.qx - cmd.qy * cmd.qy - cmd.qz * cmd.qz;
  float Rd12 = 2 * (cmd.qx * cmd.qy - cmd.qw * cmd.qz);
  float Rd13 = 2 * (cmd.qx * cmd.qz + cmd.qw * cmd.qy);
  float Rd21 = 2 * (cmd.qx * cmd.qy + cmd.qw * cmd.qz);
  float Rd22 = cmd.qw * cmd.qw - cmd.qx * cmd.qx + cmd.qy * cmd.qy - cmd.qz * cmd.qz;
  float Rd23 = 2 * (cmd.qy * cmd.qz - cmd.qw * cmd.qx);
  float Rd31 = 2 * (cmd.qx * cmd.qz - cmd.qw * cmd.qy);
  float Rd32 = 2 * (cmd.qy * cmd.qz + cmd.qw * cmd.qx);
  float Rd33 = cmd.qw * cmd.qw - cmd.qx * cmd.qx - cmd.qy * cmd.qy + cmd.qz * cmd.qz;

  float Psi = 0.5f * (3.0f - (Rd11 * R11 + Rd21 * R21 + Rd31 * R31 +
                              Rd12 * R12 + Rd22 * R22 + Rd32 * R32 +
                              Rd13 * R13 + Rd23 * R23 + Rd33 * R33));

  float force = 0;
  if (Psi < 1.0f) // Position control stability guaranteed only when Psi < 1
    force = cmd.force[0] * R13 + cmd.force[1] * R23 + cmd.force[2] * R33;

//计算旋转误差
  float eR1 = 0.5f * (R12 * Rd13 - R13 * Rd12 + R22 * Rd23 - R23 * Rd22 +
                      R32 * Rd33 - R33 * Rd32);
  float eR2 = 0.5f * (R13 * Rd11 - R11 * Rd13 - R21 * Rd23 + R23 * Rd21 -
                      R31 * Rd33 + R33 * Rd31);
  float eR3 = 0.5f * (R11 * Rd12 - R12 * Rd11 + R21 * Rd22 - R22 * Rd21 +
                      R31 * Rd32 - R32 * Rd31);


  float eOm1 = Om1;
  float eOm2 = Om2;
  float eOm3 = Om3;
  //计算三轴惯性力矩
  float in1 = Om2 * (I[2][0] * Om1 + I[2][1] * Om2 + I[2][2] * Om3) -
              Om3 * (I[1][0] * Om1 + I[1][1] * Om2 + I[1][2] * Om3);
  float in2 = Om3 * (I[0][0] * Om1 + I[0][1] * Om2 + I[0][2] * Om3) -
              Om1 * (I[2][0] * Om1 + I[2][1] * Om2 + I[2][2] * Om3);
  float in3 = Om1 * (I[1][0] * Om1 + I[1][1] * Om2 + I[1][2] * Om3) -
              Om2 * (I[0][0] * Om1 + I[0][1] * Om2 + I[0][2] * Om3);
  /*
    // Robust Control --------------------------------------------
    float c2       = 0.6;
    float epsilonR = 0.04;
    float deltaR   = 0.1;
    float eA1 = eOm1 + c2 * 1.0/I[0][0] * eR1;
    float eA2 = eOm2 + c2 * 1.0/I[1][1] * eR2;
    float eA3 = eOm3 + c2 * 1.0/I[2][2] * eR3;
    float neA = sqrt(eA1*eA1 + eA2*eA2 + eA3*eA3);
    float muR1 = -deltaR*deltaR * eA1 / (deltaR * neA + epsilonR);
    float muR2 = -deltaR*deltaR * eA2 / (deltaR * neA + epsilonR);
    float muR3 = -deltaR*deltaR * eA3 / (deltaR * neA + epsilonR);
    // Robust Control --------------------------------------------
  */
 //根据误差计算需要的控制力矩
  float M1 = -cmd.kR[0] * eR1 - cmd.kOm[0] * eOm1 + in1; // - I[0][0]*muR1;
  float M2 = -cmd.kR[1] * eR2 - cmd.kOm[1] * eOm2 + in2; // - I[1][1]*muR2;
  float M3 = -cmd.kR[2] * eR3 - cmd.kOm[2] * eOm3 + in3; // - I[2][2]*muR3;

//计算目标四个电机转速
  float w_sq[4];
  w_sq[0] = force / (4 * kf) - M2 / (2 * d * kf) + M3 / (4 * km);
  w_sq[1] = force / (4 * kf) + M2 / (2 * d * kf) + M3 / (4 * km);
  w_sq[2] = force / (4 * kf) + M1 / (2 * d * kf) - M3 / (4 * km);
  w_sq[3] = force / (4 * kf) - M1 / (2 * d * kf) - M3 / (4 * km);

  Control control_return;
  for (int i = 0; i < 4; i++)
  {
    if (w_sq[i] < 0)
      w_sq[i] = 0;

    control_return.rpm[i] = sqrtf(w_sq[i]);
  }
  //返回电机转速结构体
  return control_return;
}

#endif